#include "DAEMatrix.h"
#include <memory.h>

DAEMatrix::DAEMatrix(void)
{
	SetAsIdentity();
}

void DAEMatrix::SetCell(int row, int column, float value)
{
	m_RMData[row*4+column]=value;
}

float DAEMatrix::GetCell(int row, int column) const
{
	return m_RMData[row*4+column];
}

void DAEMatrix::SetRow(int row, DAEFloat4& value)
{

	m_RMData[row*4]   = value.x;
	m_RMData[row*4+1] = value.y;
	m_RMData[row*4+2] = value.z;
	m_RMData[row*4+3] = value.w;
}

void DAEMatrix::SetRow(int row, float a1, float a2, float a3, float a4)
{
	m_RMData[row*4]   = a1;
	m_RMData[row*4+1] = a2;
	m_RMData[row*4+2] = a3;
	m_RMData[row*4+3] = a4;
}

void DAEMatrix::SetColumn(int column, DAEFloat4& value)
{
	m_RMData[column]      = value.x;
	m_RMData[4 + column]  = value.y;
	m_RMData[8 + column]  = value.z;
	m_RMData[12 + column] = value.w;
}

void DAEMatrix::SetColumn(int column, float a1, float a2, float a3, float a4)
{
	m_RMData[column]        = a1;
	m_RMData[4 + column]   = a2;
	m_RMData[8 + column]   = a3;
	m_RMData[12 + column]  = a4;
}

void DAEMatrix::SetAsIdentity(){
	this->SetRow(0,1,0,0,0);
	this->SetRow(1,0,1,0,0);
	this->SetRow(2,0,0,1,0);
	this->SetRow(3,0,0,0,1);
}

void DAEMatrix::GetRow(int row, DAEFloat4& value) const
{
	value.x = m_RMData[row*4];
	value.y = m_RMData[row*4+1];
	value.z = m_RMData[row*4+2];
	value.w = m_RMData[row*4+3];
}

void DAEMatrix::GetColumn(int column, DAEFloat4& value) const
{
	value.x = m_RMData[column];
	value.y = m_RMData[4+column];
	value.z = m_RMData[8+column];
	value.w = m_RMData[12+column];
}

void DAEMatrix::TransformVector(DAEFloat3& vOut, DAEFloat3& vToTransform) const
{
	
	float xr = vToTransform.x * m_RMData[0] + vToTransform.y * m_RMData[4] + vToTransform.z * m_RMData[8] + m_RMData[12];
	float yr = vToTransform.x * m_RMData[1] + vToTransform.y * m_RMData[5] + vToTransform.z * m_RMData[9] + m_RMData[13];
	float zr = vToTransform.x * m_RMData[2] + vToTransform.y * m_RMData[6] + vToTransform.z * m_RMData[10] + m_RMData[14];
	float wr = vToTransform.x * m_RMData[3] + vToTransform.y * m_RMData[7] + vToTransform.z * m_RMData[11] + m_RMData[15];
	vOut.SetValues(xr/wr,yr/wr,zr/wr);
}

void DAEMatrix::TransformVector(DAEFloat4& vOut, DAEFloat3& vToTransform)const
{
	float xr = vToTransform.x * m_RMData[0] + vToTransform.y * m_RMData[4] + vToTransform.z * m_RMData[8] + m_RMData[12];
	float yr = vToTransform.x * m_RMData[1] + vToTransform.y * m_RMData[5] + vToTransform.z * m_RMData[9] + m_RMData[13];
	float zr = vToTransform.x * m_RMData[2] + vToTransform.y * m_RMData[6] + vToTransform.z * m_RMData[10] + m_RMData[14];
	float wr = vToTransform.x * m_RMData[3] + vToTransform.y * m_RMData[7] + vToTransform.z * m_RMData[11] + m_RMData[15];
	vOut.SetValues(xr,yr,zr,wr);

}

void DAEMatrix::TransformNormal(DAEFloat3& vOut, DAEFloat3& vToTransform) const
{
	// only rotation.
	float xr = vToTransform.x * m_RMData[0] + vToTransform.y * m_RMData[4] + vToTransform.z * m_RMData[8];
	float yr = vToTransform.x * m_RMData[1] + vToTransform.y * m_RMData[5] + vToTransform.z * m_RMData[9];
	float zr = vToTransform.x * m_RMData[2] + vToTransform.y * m_RMData[6] + vToTransform.z * m_RMData[10];
	vOut.SetValues(xr,yr,zr);
}

void DAEMatrix::Multiply(DAEMatrix* mOut, const DAEMatrix* toMultiply)
{
	// store the result in a temporary float buffer , otherwise
	// there are problems when the mOut matrix is the same as this
	// DAEMatrix object.
	float data[16];
	DAEFloat4 rowBuffer , columnBuffer;
	for (int row = 0; row < 4; row++)
	{
		for (int column = 0; column < 4; column++)
		{
			this->GetRow(row,rowBuffer),
			toMultiply->GetColumn(column, columnBuffer);
			data[row*4+column]=rowBuffer.Dot(columnBuffer);	
		}
	}
	// copy the result to mOut
	memcpy_s(mOut->m_RMData,16*sizeof(float),data,16*sizeof(float));

}

void DAEMatrix::GetMinor(DAEMatrix33& mOut, int row, int column)const
{
	int count = 0;
	for (int rowc = 0; rowc < 4; rowc++){
		for (int columnc =0; columnc<4; columnc++){
			if ( rowc == row || columnc==column )
				continue;
			int row33 = count/3;
			int column33 = count%3;
			mOut.SetCell(row33,column33, GetCell(rowc,columnc));
			count++;
		}
	}
}

float DAEMatrix::Determinant()const{
	DAEMatrix33 minor;
	float det=0.0f;
	for (int row = 0; row <4;row++){
		GetMinor(minor,row,0);
		det += (1-2*(row%2))*GetCell(row,0) * minor.Determinant(); 
	}
	return det;
}

void DAEMatrix::Inverse(DAEMatrix& inverse) const{
	DAEMatrix33 minor;
	float det = Determinant();
	// calculate determinants
	for (int row=0; row < 4;row++)
		for (int column=0; column<4; column++)
		{
			GetMinor(minor,column,row);
			float val = (1-2*((row+column)%2)) * minor.Determinant();
			inverse.SetCell(row,column,val/det);
		}
}